import random
import time
from robomaster import robot
from robomaster import conn
from multi_robomaster import multi_robot
from robomaster import led, blaster
from collections import deque

import pos_nokov as pos
import logging
import sympy as sp
import math
import copy
import msvcrt
from interval import Interval
from position import *
from utils import *
import center_control as cc
# import test_multi_p2 as tmp2

# field_error=False
# field_range=[[-1.33,3.04],[-1.50,1.70]]
aim_x=Interval(-1.15, 3.00)
aim_y=Interval(-1.30,1.50)
safe_x=Interval(-1.32,2.76)
safe_y=Interval(-1.42,1.61)
goal=pose(-1.4,0,180)

adjust=0.8
def_radius=0.5
def_pos=[pose(1,-0.8,0), pose(0, 1.2,0) ]
passing=0
global p, v
def led_grey_solid(robot_group):
    """grey常亮"""
    robot_group.led.set_led(led.COMP_ALL, 75, 255, 255, led.EFFECT_ON)
    
def calc_catching_pose(b, t, aim=False, catch_phase1=False):
    '''Calc pose to catch ball. b: ball pose, t: target pose'''
    x,y=sp.symbols('x, y')
    distance=0.316 if catch_phase1 else 0.5 # distance from ball to pose
    # logging.debug(f'dist:{distance}')
    solutions=sp.nonlinsolve([(x-b.x)*(b.y-t.y)-(y-b.y)*(b.x-t.x),
                              (x-b.x)**2+(y-b.y)**2-distance**2],[x,y])
    for s in solutions:
        if aim: # the catching pose aim at t
            if not (s[0] in Interval(b.x, t.x) and s[1] in Interval(b.y,t.y)):
                return pose(s[0],s[1],math.atan2(t.y-b.y, t.x-b.x)*180/math.pi)
        else: # t is agent pose and catch directly
            # logging.debug(f'sx, {s[0]},sy: {s[1]}; bx: {b.x}, by: {b.y}; tx: {t.x}, ty: {t.y}')
            if (s[0]-b.x)*(t.x-b.x)>0 and (s[1]-b.y)*(t.y-b.y)>0:
                # logging.debug(f'solved pt: x:{s[0]},y:{s[1]}')
                return pose(s[0],s[1],math.atan2(b.y-t.y, b.x-t.x)*180/math.pi)
    
class EP_def:
    def __init__(self, multi_robots, robot_group, id, role='stand_by'):
        global def_pos, def_radius
        self.role=role
        self.multi_robots=multi_robots
        self.chase_phase=0
        self.catch_phase=0
        self.pass_phase=0
        self.close_to_oppo=False
        self.robot_group=robot_group
        self.id=id
        if self.id==5:
            self.def_pos=def_pos[0]
            print(f'ep{self.id} def_pos is: {self.def_pos}')
        elif self.id==6:
            self.def_pos=def_pos[1]
            print(f'ep{self.id} def_pos is: {self.def_pos}')
        self.def_radius=def_radius
        self.aim_to_pass=False
        self.trans_pt=pose(0.5,0.5,5)
        if self.role=='stand_by':
            cc.sb.append(self.id)
        
    def init_arm(self, rg):
        rg.robotic_arm.moveto(0, -70).wait_for_completed()
        rg.gripper.open(100)
        time.sleep(2)
        
    def led_red_solid(self, rg):
        """红灯常亮"""
        rg.led.set_led(led.COMP_ALL, 255, 1, 1, led.EFFECT_ON)
    
    def init_pos(self,rg):
        global p, v
        v_best=get_rvo_vel(p.get_arr(), v.get_arr(),cc.pt.get_arr())
        self.trans_pt=trans_relative_co(pose(0,0,p[self.id].phi), pose(v_best[self.id].x,v_best[self.id].y))
        phi_pt=limit_pi(cc.pt[self.id].phi-p[self.id].phi)
        if dis_pose(cc.pt[self.id], p[self.id])<0.1 and phi_pt<3:
            logging.debug(f'ep{self.id} current:{p[self.id]},target:{cc.pt[self.id]}')
            return True
        self.trans_pt.update(self.trans_pt.x, self.trans_pt.y, phi_pt)
        rg.chassis.drive_speed(self.trans_pt.x, -self.trans_pt.y/2, -45*(math.tanh(self.trans_pt.phi/45)), timeout=0.5)
        time.sleep(0.1)
        return False
        
    def stop(self, rg):
        rg.chassis.drive_wheels(w1=0, w2=0, w3=0, w4=0,timeout=1)
        time.sleep(0.01)
    
    def defend(self, rg):
        global p
        cc.pt[self.id]=calc_catching_pose(p[0], p[self.id], False, self.chase_phase==1)
        # self.trans_pt=trans_relative_co(p[self.id], cc.pt[self.id]) # 获得机器人坐标系下的目标坐标pt
        # v=np.array([self.trans_pt.x, self.trans_pt.y])
        # v_clipped=v/get_len(v[0],v[1])*0.1
        # phi_pt=limit_pi(cc.pt[self.id].phi-p[self.id].phi)
        # self.trans_pt.update(v_clipped[0], v_clipped[1], phi_pt)
        # rg.chassis.drive_speed(self.trans_pt.x, self.trans_pt.y, -10*(math.tanh(self.trans_pt.phi/45)), timeout=1)
        # time.sleep(0.1)
    
    def defend_simple(self, rg):
        global p
        # if dis_pose(p[self.id],self.def_pos)<self.def_radius:
        #     cc.pt[self.id]=calc_catching_pose(p[0], p[self.id], False, self.chase_phase==1)
        # else:
        #     # 若超范围，则退回至当前与防守点的中间点
        cc.pt[self.id]=self.def_pos
        self.trans_pt=trans_relative_co(p[self.id], cc.pt[self.id]) # 获得机器人坐标系下的目标坐标pt   
        v=np.array([self.trans_pt.x, self.trans_pt.y])
        v_clipped=v/get_len(v[0],v[1])*0.1
        self.trans_pt.update(v_clipped[0], v_clipped[1], 0)
        rg.chassis.drive_speed(self.trans_pt.x, self.trans_pt.y, -10*(math.tanh(self.trans_pt.phi/45)), timeout=1)
        time.sleep(0.1)
        
    def run(self,gp, gv):
        logging.basicConfig(format='%(asctime)s.%(msecs)03d-[line:%(lineno)d]'
                            '-%(levelname)s: %(message)s',
                            level=logging.DEBUG, datefmt='%H:%M:%S')
        global passing, p, v
        self.multi_robots.run([self.robot_group, self.led_red_solid])

        # cc.pt=copy.deepcopy(p)
        cc.pt[self.id]=self.def_pos
        cc.ready[self.id]=0
        while(True):
            p=gp
            v=gv
            if self.multi_robots.run([self.robot_group, self.init_pos]):
                break
            
        logging.info(f'ep{self.id} reached init pos')
        self.multi_robots.run([self.robot_group, self.stop])
        cc.ready[self.id]=1
        while 0 in cc.ready:
            time.sleep(0.2)
        time.sleep(0.5)
        while not pos.done:
            p=gp
            v=gv
            try:
                self.multi_robots.run([self.robot_group, self.defend])
            except Exception as e:
                logging.error(f'Exception: {e}')
                break
            
            if (msvcrt.kbhit() and msvcrt.getch() == b'p'):
                print("ep %d Paused..." % self.id)
                while msvcrt.getch() != b'r':
                    time.sleep(0.5)
                    msvcrt.getch()
                print("ep %d resumed..." % self.id)
            if msvcrt.kbhit() and msvcrt.getch() == b't':
                break    
        pos.done=True
        print('over...')
        self.multi_robots.close()
